/**
 * @file ebex_vector.h
 *
 * @date Sep 14, 2011
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2011 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#ifndef EBEX_VECTOR_H_
#define EBEX_VECTOR_H_

#include <assert.h>

#include <ebex_math.h>
#include <ebex_lookup.h>

#define _rotation_orders(x,_) 	\
		_(x,XYZ)				\
		_(x,XZY)				\
		_(x,YXZ)				\
		_(x,YZX)				\
		_(x,ZXY)				\
		_(x,ZYX)

EBEX_LOOKUP_TABLE(rotation_order, static);

typedef struct rotation_order_t
{
	int axis[3];	//!< axis index order
	int parity;		//!< parity of axis permutation (even=0, odd=1)
} rotation_order_t;

static const rotation_order_t rot_order[] =
	{
			/// { i, j, k }, n
			{
				{ 0, 1, 2 }, 0 }, /// XYZ
			{
				{ 0, 2, 1 }, 1 }, /// XZY
			{
				{ 1, 0, 2 }, 1 }, /// YXZ
			{
				{ 1, 2, 0 }, 0 }, /// YZX
			{
				{ 2, 0, 1 }, 0 }, /// ZXY
			{
				{ 2, 1, 0 }, 1 }  /// ZYX
	};

#define GET_ROT_ORDER(order) (assert(order>=0 && order<rotation_order_tblEnd), &rot_order[(order)])

void ebex_3vec_make_compatible_euler(ebex_3vec_t *m_euler, const ebex_3vec_t *m_src);
void ebex_3vec_euler_from_3mat(ebex_3vec_t *m_dest, e_rotation_order m_order, const ebex_mat3x3_t *m_src);
void ebex_3vec_euler_from_3mat_ref(ebex_3vec_t *m_dest, ebex_3vec_t *m_ref, e_rotation_order m_order, const ebex_mat3x3_t *m_src);
void ebex_3vec_euler_from_quat(ebex_3vec_t *m_dest, e_rotation_order m_order, const ebex_quat_t *m_src);

static inline void ebex_3vec_copy( ebex_3vec_t *m_result, const ebex_3vec_t *m_vec )
{
    m_result->x = m_vec->x;
    m_result->y = m_vec->y;
    m_result->z = m_vec->z;
}

static inline void ebex_3vec_make_from_elems( ebex_3vec_t *m_result, double _x, double _y, double _z )
{
    m_result->x = _x;
    m_result->y = _y;
    m_result->z = _z;
}

static inline void ebex_3vec_make_from_scalar( ebex_3vec_t *m_result, double m_scalar )
{
    m_result->x = m_scalar;
    m_result->y = m_scalar;
    m_result->z = m_scalar;
}

static inline void ebex_3vec_add( ebex_3vec_t *m_result, const ebex_3vec_t *m_vec0, const ebex_3vec_t *m_vec1 )
{
    m_result->x = ( m_vec0->x + m_vec1->x );
    m_result->y = ( m_vec0->y + m_vec1->y );
    m_result->z = ( m_vec0->z + m_vec1->z );
}

static inline void ebex_3vec_sub( ebex_3vec_t *m_result, const ebex_3vec_t *m_vec0, const ebex_3vec_t *m_vec1 )
{
    m_result->x = ( m_vec0->x - m_vec1->x );
    m_result->y = ( m_vec0->y - m_vec1->y );
    m_result->z = ( m_vec0->z - m_vec1->z );
}

static inline void ebex_3vec_scalar_mul( ebex_3vec_t *m_result, const ebex_3vec_t *m_vec, double m_scalar )
{
    m_result->x = ( m_vec->x * m_scalar );
    m_result->y = ( m_vec->y * m_scalar );
    m_result->z = ( m_vec->z * m_scalar );
}

static inline void ebex_3vec_scalar_div( ebex_3vec_t *m_result, const ebex_3vec_t *m_vec, double m_scalar )
{
    m_result->x = ( m_vec->x / m_scalar );
    m_result->y = ( m_vec->y / m_scalar );
    m_result->z = ( m_vec->z / m_scalar );
}

static inline void ebex_3vec_neg( ebex_3vec_t *m_result, const ebex_3vec_t *m_vec )
{
    m_result->x = -m_vec->x;
    m_result->y = -m_vec->y;
    m_result->z = -m_vec->z;
}

static inline void ebex_3vec_mul_per_elem( ebex_3vec_t *m_result, const ebex_3vec_t *m_vec0, const ebex_3vec_t *m_vec1 )
{
    m_result->x = ( m_vec0->x * m_vec1->x );
    m_result->y = ( m_vec0->y * m_vec1->y );
    m_result->z = ( m_vec0->z * m_vec1->z );
}

static inline void ebex_3vec_div_per_elem( ebex_3vec_t *m_result, const ebex_3vec_t *m_vec0, const ebex_3vec_t *m_vec1 )
{
    m_result->x = ( m_vec0->x / m_vec1->x );
    m_result->y = ( m_vec0->y / m_vec1->y );
    m_result->z = ( m_vec0->z / m_vec1->z );
}

static inline void ebex_3vec_recip_per_elem( ebex_3vec_t *m_result, const ebex_3vec_t *m_vec )
{
    m_result->x = ( 1.0 / m_vec->x );
    m_result->y = ( 1.0 / m_vec->y );
    m_result->z = ( 1.0 / m_vec->z );
}

static inline void ebex_3vec_sqrt_per_elem( ebex_3vec_t *m_result, const ebex_3vec_t *m_vec )
{
    m_result->x = sqrt( m_vec->x );
    m_result->y = sqrt( m_vec->y );
    m_result->z = sqrt( m_vec->z );
}

static inline void ebex_3vec_rsqrt_per_elem( ebex_3vec_t *m_result, const ebex_3vec_t *m_vec )
{
    m_result->x = ( 1.0 / sqrt( m_vec->x ) );
    m_result->y = ( 1.0 / sqrt( m_vec->y ) );
    m_result->z = ( 1.0 / sqrt( m_vec->z ) );
}

static inline void ebex_3vec_abs_per_elem( ebex_3vec_t *m_result, const ebex_3vec_t *m_vec )
{
    m_result->x = fabs( m_vec->x );
    m_result->y = fabs( m_vec->y );
    m_result->z = fabs( m_vec->z );
}

static inline void ebex_3vec_copy_sign_per_elem( ebex_3vec_t *m_result, const ebex_3vec_t *m_vec0, const ebex_3vec_t *m_vec1 )
{
    m_result->x = ( m_vec1->x < 0.0 )? -fabs( m_vec0->x ) : fabs( m_vec0->x );
    m_result->y = ( m_vec1->y < 0.0 )? -fabs( m_vec0->y ) : fabs( m_vec0->y );
    m_result->z = ( m_vec1->z < 0.0 )? -fabs( m_vec0->z ) : fabs( m_vec0->z );
}

static inline void ebex_3vec_max_per_elem( ebex_3vec_t *m_result, const ebex_3vec_t *m_vec0, const ebex_3vec_t *m_vec1 )
{
    m_result->x = (m_vec0->x > m_vec1->x)? m_vec0->x : m_vec1->x;
    m_result->y = (m_vec0->y > m_vec1->y)? m_vec0->y : m_vec1->y;
    m_result->z = (m_vec0->z > m_vec1->z)? m_vec0->z : m_vec1->z;
}

static inline double ebex_3vec_max_elem( const ebex_3vec_t *m_vec )
{
    double m_result;
    m_result = (m_vec->x > m_vec->y)? m_vec->x : m_vec->y;
    m_result = (m_vec->z > m_result)? m_vec->z : m_result;
    return m_result;
}

static inline void ebex_3vec_min_per_elem( ebex_3vec_t *m_result, const ebex_3vec_t *m_vec0, const ebex_3vec_t *m_vec1 )
{
    m_result->x = (m_vec0->x < m_vec1->x)? m_vec0->x : m_vec1->x;
    m_result->y = (m_vec0->y < m_vec1->y)? m_vec0->y : m_vec1->y;
    m_result->z = (m_vec0->z < m_vec1->z)? m_vec0->z : m_vec1->z;
}

static inline double ebex_3vec_min_elem( const ebex_3vec_t *m_vec )
{
    double m_result;
    m_result = (m_vec->x < m_vec->y)? m_vec->x : m_vec->y;
    m_result = (m_vec->z < m_result)? m_vec->z : m_result;
    return m_result;
}

static inline void ebex_3vec_make_xaxis( ebex_3vec_t *m_result )
{
    ebex_3vec_make_from_elems( m_result, 1.0, 0.0, 0.0 );
}

static inline void ebex_3vec_make_yaxis( ebex_3vec_t *m_result )
{
    ebex_3vec_make_from_elems( m_result, 0.0, 1.0, 0.0 );
}

static inline void ebex_3vec_make_zaxis( ebex_3vec_t *m_result )
{
    ebex_3vec_make_from_elems( m_result, 0.0, 0.0, 1.0 );
}

static inline void ebex_3vec_set_x( ebex_3vec_t *m_result, double _x )
{
    m_result->x = _x;
}

static inline double ebex_3vec_get_x( const ebex_3vec_t *m_vec )
{
    return m_vec->x;
}

static inline void ebex_3vec_set_y( ebex_3vec_t *m_result, double _y )
{
    m_result->y = _y;
}

static inline double ebex_3vec_get_y( const ebex_3vec_t *m_vec )
{
    return m_vec->y;
}

static inline void ebex_3vec_set_z( ebex_3vec_t *m_result, double _z )
{
    m_result->z = _z;
}

static inline double ebex_3vec_get_z( const ebex_3vec_t *m_vec )
{
    return m_vec->z;
}

static inline void ebex_3vec_set_elem( ebex_3vec_t *m_result, int idx, double value )
{
    m_result->element[idx] = value;
}

static inline double ebex_3vec_get_elem( const ebex_3vec_t *m_vec, int idx )
{
    return m_vec->element[idx];
}

static inline double ebex_3vec_sum( const ebex_3vec_t *m_vec )
{
    double m_result;
    m_result = ( m_vec->x + m_vec->y );
    m_result = ( m_result + m_vec->z );
    return m_result;
}

static inline double ebex_3vec_dot( const ebex_3vec_t *m_vec0, const ebex_3vec_t *m_vec1 )
{
    double m_result[3];
    m_result[0] = ( m_vec0->x * m_vec1->x );
    m_result[1] = ( m_vec0->y * m_vec1->y );
    m_result[2] = ( m_vec0->z * m_vec1->z );
    return m_result[0] + m_result[1] + m_result[2];
}

static inline double ebex_3vec_length_sqr( const ebex_3vec_t *m_vec )
{
    double m_result[3];
    m_result[0] = ( m_vec->x * m_vec->x );
    m_result[1] = ( m_vec->y * m_vec->y );
    m_result[2] = ( m_vec->z * m_vec->z );
    return m_result[0] + m_result[1] + m_result[2];
}

static inline double ebex_3vec_length( const ebex_3vec_t *m_vec )
{
    return sqrt( ebex_3vec_length_sqr( m_vec ) );
}

static inline void ebex_3vec_normalize( ebex_3vec_t *m_result, const ebex_3vec_t *m_vec )
{
    double len_sqr, len_inv;
    len_sqr = ebex_3vec_length_sqr( m_vec );
    len_inv = ( 1.0 / sqrt( len_sqr ) );
    m_result->x = ( m_vec->x * len_inv );
    m_result->y = ( m_vec->y * len_inv );
    m_result->z = ( m_vec->z * len_inv );
}

static inline void ebex_3vec_cross( ebex_3vec_t *m_result, const ebex_3vec_t *m_vec0, const ebex_3vec_t *m_vec1 )
{
    double tmp_x, tmp_y, tmp_z;
    tmp_x = ( ( m_vec0->y * m_vec1->z ) - ( m_vec0->z * m_vec1->y ) );
    tmp_y = ( ( m_vec0->z * m_vec1->x ) - ( m_vec0->x * m_vec1->z ) );
    tmp_z = ( ( m_vec0->x * m_vec1->y ) - ( m_vec0->y * m_vec1->x ) );
    ebex_3vec_make_from_elems( m_result, tmp_x, tmp_y, tmp_z );
}

static inline void ebex_3vec_lerp( ebex_3vec_t *m_result, double m_fraction, const ebex_3vec_t *m_vec0, const ebex_3vec_t *m_vec1 )
{
    ebex_3vec_t tmp_v3_0, tmp_v3_1;
    ebex_3vec_sub( &tmp_v3_0, m_vec1, m_vec0 );
    ebex_3vec_scalar_mul( &tmp_v3_1, &tmp_v3_0, m_fraction );
    ebex_3vec_add( m_result, m_vec0, &tmp_v3_1 );
}

static inline void ebex_3vec_slerp( ebex_3vec_t *m_result, double m_fraction, const ebex_3vec_t *unit_vec0, const ebex_3vec_t *unit_vec1 )
{
    ebex_3vec_t tmp_v3_0, tmp_v3_1;
    double recip_sin_angle, scale0, scale1, cos_angle, angle;
    cos_angle = ebex_3vec_dot( unit_vec0, unit_vec1 );
    if ( cos_angle < ebex_slerp_tol ) {
        angle = acos( cos_angle );
        recip_sin_angle = ( 1.0 / sin( angle ) );
        scale0 = ( sin( ( ( 1.0 - m_fraction ) * angle ) ) * recip_sin_angle );
        scale1 = ( sin( ( m_fraction * angle ) ) * recip_sin_angle );
    } else {
        scale0 = ( 1.0 - m_fraction );
        scale1 = m_fraction;
    }
    ebex_3vec_scalar_mul( &tmp_v3_0, unit_vec0, scale0 );
    ebex_3vec_scalar_mul( &tmp_v3_1, unit_vec1, scale1 );
    ebex_3vec_add( m_result, &tmp_v3_0, &tmp_v3_1 );
}



#endif /* EBEX_VECTOR_H_ */
